#!/usr/bin/env python3

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # Declare launch arguments
    rviz_arg = DeclareLaunchArgument(
        'rviz',
        default_value='true',
        description='Whether to start RViz'
    )
    
    # Get the package directory
    pkg_point_lio = get_package_share_directory('point_lio')
    
    # Path to config file
    config_file = os.path.join(pkg_point_lio, 'config', 'horizon.yaml')
    
    # Point-LIO mapping node
    pointlio_mapping_node = Node(
        package='point_lio',
        executable='pointlio_mapping',
        name='laserMapping',
        output='screen',
        parameters=[
            config_file,
            {
                'use_imu_as_input': False,
                'prop_at_freq_of_imu': True,
                'check_satu': True,
                'init_map_size': 10,
                'point_filter_num': 3,
                'space_down_sample': True,
                'filter_size_surf': 0.5,
                'filter_size_map': 0.5,
                'ivox_nearby_type': 6,
                'runtime_pos_log_enable': False
            }
        ]
    )
    
    # RViz node
    rviz_config = os.path.join(pkg_point_lio, 'rviz_cfg', 'loam_livox.rviz')
    rviz_node = GroupAction(
        condition=IfCondition(LaunchConfiguration('rviz')),
        actions=[
            Node(
                package='rviz2',
                executable='rviz2',
                name='rviz2',
                arguments=['-d', rviz_config],
                output='screen'
            )
        ]
    )
    
    return LaunchDescription([
        rviz_arg,
        pointlio_mapping_node,
        rviz_node
    ]) 